#include "communication/typetransfer.h"
#include "masters/arm_controller.h"
#include "main_controller.h"
#include "masters/vehicle_controller.h"
#include "communication/utility.hpp"
#include "predef.h"
#include "utils.h"
#include "mainwindow.h"
#include "Eigen/Eigen"
#include <predef.h>
#include <communication/utility.hpp>
using namespace Eigen;
using namespace main_controller;
extern bool global_stop_flag;


void MainController::RecvMsgFromMasterDevice(const u_char master_index, const QVector<double> &data)
{
    if(master_index!=MasterIndex::kLeftDeviceIndex && master_index!=MasterIndex::kRightDeviceIndex)
        return;

    for (size_t i=0;i<7;++i)
    {
        master_device_data_[master_index][i]=data[i];
    }
    cout<<master_names[master_index]<<" master device updated"<<target_robot_index_<<endl;
//    switch (slave_index)
//    {
//    case SlaveIndex::kLeftArmHandIndex:
//        cout<<"left arm: ";
//        break;
//    case SlaveIndex::kRightArmHandIndex:
//        cout<<"right arm: ";
//        break;
//    default:
//        cout<<"unknown part: ";
//        return;
//    }

//    for (int i=0;i<data.size();i++) {
//        cout<<data[i]<<" ";
//    }
//    cout<<endl;

//    if(data.size()!=7)//6 descart data + hand data
//        return;
//    //arm_controller_->SetHandTargetJointPos(data[6]);
//    ArmControllerPtr controller=boost::dynamic_pointer_cast<ArmController>(master_controllers_[slave_index]);

//    controller->SetArmTargetEndPos(data[0],data[1],data[2]);

//    Eigen::Matrix3d a;
//    a=AngleAxisd(data[3],Eigen::Vector3d::UnitX())* AngleAxisd(data[4], Vector3d::UnitY())* AngleAxisd(data[5], Vector3d::UnitZ());
//    controller->SetArmTargetEndQuat(Quaterniond(a));

//    controller->SetHandTargetJointPos(data[6]);
}

MainController::MainController(MainWindow *main_window_)
{
    connect(main_window_,&MainWindow::MasterDeviceUpdate,this,&MainController::RecvMsgFromMasterDevice);

    connect(main_window_,&MainWindow::ConnectSocket,this,&MainController::OnConnectSocketCallback);
    connect(main_window_,&MainWindow::SetFreq,this,&MainController::OnSetFreqCallback);
    master_device_data_.resize(2);
    master_device_data_[0].resize(7,0.0);
    master_device_data_[1].resize(7,0.0);
    target_robot_index_=-1;
}

MainController::~MainController()
{
}
void MainController::OnConnectSocketCallback(const QString &addr, const unsigned short port)
{
    endpoint_=tcp::endpoint(address::from_string(addr.toStdString()), port);
    socket_client_.reset(new AsynSocketClient(this,io_service_,endpoint_));
    socket_client_->Start();

    cout<<"connect: "<<addr.toStdString()<<" : "<<port<<endl;
}

void MainController::OnSetFreqCallback(const unsigned int freq)
{
    control_duration_ms_=max(10,min(2000,int(1000.0/(double)freq)));
    cout<<"control duration: "<<(int)control_duration_ms_<<" ms"<<endl;
}

void MainController::Init()
{
    endpoint_=tcp::endpoint(address::from_string("127.0.0.1"), 48888);
    socket_client_.reset(new AsynSocketClient(this,io_service_,endpoint_));
    socket_client_->Start();

    control_duration_ms_=50;

    LoadParams();

    working_status_=kRCStop;
    connection_status_=kConnection2MonitorLost;

    left_master_device_id_=0;
    right_master_device_id_=1;

    BaseControllerPtr left_armhand_controller(new ArmController(this,kLeftArmHandIndex));
    BaseControllerPtr right_armhand_controller(new ArmController(this,kRightArmHandIndex));
    //BaseControllerPtr vehicle_controller(new VehicleController(this,20,M_PI_2));
    left_armhand_controller->AllocMasterDevice(&left_master_device_id_);
    master_controllers_.push_back(left_armhand_controller);
    master_controllers_.push_back(right_armhand_controller);
    //master_controllers_.push_back(vehicle_controller);

}


void MainController::Execute()
{
    boost::asio::deadline_timer timer(io_service_, boost::posix_time::microsec(control_duration_ms_*1000));
    timer.async_wait(boost::bind(&MainController::TimerUpdate,this, boost::asio::placeholders::error,&timer));
    io_service_.run();
}

void MainController::Stop()
{
    working_status_=main_controller::RunningStatusType::kRCStop;
}

bool MainController::LoadParams()
{
    return true;
}

bool MainController::RecvFrameData(const u_char *data, const size_t len)
{
    cout<<"recv socket frame data: "<<Uchar2HexQstring(data,len).toStdString()<<endl;
    QString str= Uchar2HexQstring(data,len);
    RecvMsgFromMonitor(str);

    u_int16_t data_len=0;
    TypeTransfer<u_int16_t>::ByteArray2Type(data_len,data+3);

    u_int16_t msg_id=0;
    TypeTransfer<u_int16_t>::ByteArray2Type(msg_id,data+5);

    int32_t robot_id=0;
    TypeTransfer<int32_t>::ByteArray2Type(robot_id,data+7);

    connection_status_=kConnectionOkay;
    switch (msg_id)
    {
    case Monitor2RC_Start:
        if(data_len!=10)    //wrong frame
            return false;
        if(working_status_!=kRCStop)
            return false;
        target_robot_index_=robot_id;
        working_status_=kRCWorking;
        StartRobotRC();
        emit UpdateStatusInfo(0,QString::number(target_robot_index_));
        emit UpdateStatusInfo(1,"start");

        {
            u_char left_device_target_part=0;
            u_char right_device_target_part=0;
            TypeTransfer<u_char>::ByteArray2Type(left_device_target_part,data+11);
            TypeTransfer<u_char>::ByteArray2Type(right_device_target_part,data+12);

            for(auto iter : master_controllers_)
            {
                BaseController &p=*iter;
                if(left_device_target_part==p.GetSlaveTargetIndex())
                {
                    p.AllocMasterDevice(&left_master_device_id_);
                    cout<<"left master device ->"<<slave_names[p.GetSlaveTargetIndex()]<<endl;
                    emit UpdateStatusInfo(2,slave_names[p.GetSlaveTargetIndex()]);
                }
                else if (right_device_target_part==p.GetSlaveTargetIndex())
                {
                    p.AllocMasterDevice(&right_master_device_id_);
                    cout<<"right master device ->"<<slave_names[p.GetSlaveTargetIndex()]<<endl;
                    emit UpdateStatusInfo(3,slave_names[p.GetSlaveTargetIndex()]);
                }
                else
                {
                    p.RemoveMasterDevice();
                    cout<<"remove master device from "<<slave_names[p.GetSlaveTargetIndex()]<<endl;
                }
            }
        }
        break;

    case Monitor2RC_Pause:
        if(data_len!=9)    //wrong frame
            return false;
        if(working_status_!=kRCWorking)
            return false;
        if(target_robot_index_!=robot_id)
            return false;
        working_status_=kRCPause;
        cout<<"robot"<<target_robot_index_<<" pauses."<<endl;
        emit UpdateStatusInfo(1,"pause");
        break;

    case Monitor2RC_Resume:
        if(data_len!=9)    //wrong frame
            return false;
        if(working_status_==kRCPause && robot_id==target_robot_index_)
        {
            working_status_=kRCWorking;
            cout<<"robot"<<target_robot_index_<<" resumes."<<endl;
            emit UpdateStatusInfo(1,"resume");
        }
        else
        {
            return false;
        }
        break;

    case Monitor2RC_Stop:
        if(data_len!=9)    //wrong frame
            return false;
        if(working_status_!=kRCStop && robot_id==target_robot_index_ )
        {
            working_status_=kRCStop;
            StopRobotRC();
            emit UpdateStatusInfo(1,"stop");
        }
        else
        {
            return false;
        }
        break;

    case Monitor2RC_Switch:
        if(data_len!=10)    //wrong frame
            return false;
        if(working_status_!=kRCStop && robot_id==target_robot_index_ )
        {
            u_char master_device_index=0,slave_target_index=0;
            TypeTransfer<u_char>::ByteArray2Type(master_device_index,data+11);
            TypeTransfer<u_char>::ByteArray2Type(slave_target_index,data+12);

//            if(master_device_index!=kLeftDeviceIndex && master_device_index!=kRightDeviceIndex)
//                return false;
            if(slave_target_index!=kLeftArmHandIndex && slave_target_index!=kRightArmHandIndex && slave_target_index!=kVehicleIndex)
                return false;

            for (auto iter : master_controllers_)
            {
                BaseController &p=*iter;
                if(p.GetSlaveTargetIndex()==slave_target_index)
                {
                    switch (master_device_index)
                    {
                    case kLeftDeviceIndex:
                        p.AllocMasterDevice(&left_master_device_id_);
                        cout<<"left master device ->"<<slave_names[p.GetSlaveTargetIndex()]<<endl;
                        emit UpdateStatusInfo(2,slave_names[p.GetSlaveTargetIndex()]);
                        break;
                    case kRightDeviceIndex:
                        p.AllocMasterDevice(&right_master_device_id_);
                        cout<<"right master device ->"<<slave_names[p.GetSlaveTargetIndex()]<<endl;
                        emit UpdateStatusInfo(3,slave_names[p.GetSlaveTargetIndex()]);
                        break;
                    default:
                        p.RemoveMasterDevice();
                        cout<<"remove master deive from "<<slave_names[p.GetSlaveTargetIndex()]<<endl;
                        break;
                    }

                }
                else if(p.GetMasterDeviceIndex()==master_device_index)
                {
                    p.RemoveMasterDevice();
                    cout<<"remove master deive from "<<slave_names[p.GetSlaveTargetIndex()]<<endl;
                }
            }
        }
        else
        {
            return false;
        }

        break;

    case Monitor2RC_Robotlost:
        //TODO
        if(target_robot_index_==robot_id)
            connection_status_=SocketConnectionStatusType::kConnection2RobotLost;
        break;
    case Robot2RC_Feedback_ArmHand:
    {
        cout<<"recv feedback from robot arm"<<target_robot_index_<<endl;
        connection_status_=kConnectionOkay;
        u_char part=0;
        TypeTransfer<u_char>::ByteArray2Type(part,data+11);

        for(auto iter : master_controllers_)
        {
            if(iter->GetSlaveTargetIndex()==part)
            {
                iter->ParseUsrData(data+11,data_len+5-11-2);
                return true;
            }
        }
        return false;
        //usr_len+head_len-useless_len-crc_len
    }
        break;
    case Robot2RC_Feedback_Head:
        cout<<"recv feedback from robot head"<<target_robot_index_<<endl;
        connection_status_=kConnectionOkay;
        break;
    case Robot2RC_Feedback_Vehicle:
        cout<<"recv feedback from robot vehicle"<<target_robot_index_<<endl;
        connection_status_=kConnectionOkay;
        break;
    default:
        break;
    }

    //cout<<"now status is "<<working_status_<<endl;
    return true;
}

void MainController::SendFrameData(const u_char *data,const size_t len)//msg_id+robot_id+controller.PackUserData
{
    socket_client_->Write(data,len);
}

void MainController::TimerUpdate(const boost::system::error_code& error,boost::asio::deadline_timer* timer)
{
    if(global_stop_flag)
    {
        cout<<"timer thread terminated"<<endl;
        return;
    }

    timer->expires_at(timer->expires_at() + boost::posix_time::microsec(control_duration_ms_*1000));
    timer->async_wait(boost::bind(&MainController::TimerUpdate,this, boost::asio::placeholders::error,timer));

    if(error)
    {
        cout<<"timer error"<<endl;
        return;
    }
    for (auto iter=master_controllers_.begin();iter!=master_controllers_.end();iter++)
    {
        BaseControllerPtr controller=*iter;
        controller->Update();
        u_char temp[kSocketBufferLen];
        size_t len=0;
        bool ret=controller->PackFrameData(temp,len);
        if(ret)
        {
            SendFrameData(temp,len);
            //SendMsgToRobot(controller->GetSlaveTargetIndex(),str);
            cout<<master_names[controller->GetMasterDeviceIndex()]
                    <<" send msg to robot"<<target_robot_index_<<"'s "<<slave_names[controller->GetSlaveTargetIndex()]<<" via monitor:"
                   <<Uchar2HexQstring(temp,len).toStdString().data()<<endl;
        }
    }
}


void MainController::StartRobotRC()
{
    u_char write_buffer[14]={0};
    write_buffer[0]=SunlinkProtocal::kSunlinkHeader1;
    write_buffer[1]=SunlinkProtocal::kSunlinkHeader2;
    write_buffer[2]=SunlinkProtocal::kSunlinkVersion;

    write_buffer[3]=9;//len
    write_buffer[4]=0;

    write_buffer[5]=RC2Robot_Command%256;//msg_id
    write_buffer[6]=RC2Robot_Command/256;

    TypeTransfer<int32_t>::Type2ByteArray(write_buffer+7,GetTargetRobotIndex());//robot_id

    write_buffer[11]=2;//0 manual mode, 1 action mode, 2 remote mode

    u_int16_t crc=CalCRC16(write_buffer,12);
    TypeTransfer<u_int16_t>::Type2ByteArray(write_buffer+12,crc);//crc

    SendFrameData(write_buffer,14);
    cout<<"robot"<<GetTargetRobotIndex()<< " starts, send msg to robot: "<<Uchar2HexQstring(write_buffer,14).toStdString()<<endl;
}

void MainController::StopRobotRC()
{
    u_char write_buffer[14]={0};
    write_buffer[0]=SunlinkProtocal::kSunlinkHeader1;
    write_buffer[1]=SunlinkProtocal::kSunlinkHeader2;
    write_buffer[2]=SunlinkProtocal::kSunlinkVersion;

    write_buffer[3]=9;//len
    write_buffer[4]=0;

    write_buffer[5]=RC2Robot_Command%256;//msg_id
    write_buffer[6]=RC2Robot_Command/256;

    TypeTransfer<int32_t>::Type2ByteArray(write_buffer+7,GetTargetRobotIndex());//robot_id

    write_buffer[11]=1;//0 manual mode, 1 action mode, 2 remote mode

    u_int16_t crc=CalCRC16(write_buffer,12);
    TypeTransfer<u_int16_t>::Type2ByteArray(write_buffer+12,crc);//crc
    SendFrameData(write_buffer,14);
    cout<<"robot"<<GetTargetRobotIndex()<< " stops, send msg to robot: "<<Uchar2HexQstring(write_buffer,14).toStdString()<<endl;

}
